/**
* @file recorder_task.c in InsCubeBsp
* @author linfe
* @comment
* Create on 2022/9/15 23:23
* @version 1.0
**/


#include "recorder_task.h"
#include <stdio.h>
#include <math.h>
_Noreturn void recorder_task(void *p) {
    recorder_task_para_t *task = (recorder_task_para_t *) p;
    char *task_name = pcTaskGetName(NULL);
    task->trace("%s: task start\n", task_name);
    if (!task->error_handler) {
        for (;;) {
            bsp_delay_ms(0xFFFFFF);
        }
    }
    if (!task->recorder) {
        task->recorder = app_recorder_create(NULL);
        task->recorder->trace = task->trace; /*DO NOT be NULL!*/
    }
    app_recorder_error_t res = app_recorder_init(task->recorder);
    if (res != app_recorder_ok) {
        task->error_handler("app recorder init failed\n");
    }
    app_recorder_error_t recorder_error = app_recorder_init(task->recorder);
    if (recorder_error != app_recorder_ok) {
        task->trace("%s: recorder init failed(%d)\n", task_name, recorder_error);
        task->error_handler("recorder init failed\n");
    }
    task->trace("%s: log to %s\n", task_name, task->recorder->filename);
    recorder_data_t data;
    uint32_t data_cnt = 0;
    system_time_t tm_pre = {0};
    while (1) {
        if (bsp_msg_get(task->recorder_queue, (uint8_t *) &data, 0x01) == pdTRUE) {
            if (app_recorder_push(task->recorder, (uint8_t *) &data, sizeof(data)) != app_recorder_ok) {
                task->error_handler("app recorder push failed\n");
            }
            system_time_t tm = data.tm;
            float delta_t = (float) ((int32_t) tm.millisecond - (int32_t) tm_pre.millisecond) * 1e-3f
                + (float) ((int32_t) tm.microsecond - (int32_t) tm_pre.microsecond) * 1e-6f;
            if (fabsf(delta_t) > 2 / 1024.0) {
//                task->trace("delta_t is too large %d *1e-6\n", (int) (1e6 * delta_t));
            }
            tm_pre = tm;
            data_cnt++;
        }
    }
}